import sys
import os
from time import *
from flask import Flask

sys.path.append("..")
from scservo_sdk import *

app = Flask(__name__)


class ServoController:

    def __init__(self, port="COM3", baudrate=1000000):
        """
        初始化舵机，打开端口，ping舵机是否在线
        :param port: 串口号
        :param baudrate: 波特率
        """
        self.port = port
        self.baudrate = baudrate
        self.servo_id_1 = 1
        self.servo_id_2 = 2
        self.portHandler = PortHandler(self.port)
        self.packetHandler = sms_sts(self.portHandler)
        print("=============正在尝试打开端口=============")
        if self.portHandler.openPort():
            print("成功打开端口")
        else:
            print("未能打开端口")
        print("=============正在检查舵机在线状态=============")
        if self.ping(self.servo_id_1) and self.ping(self.servo_id_2):
            print("两个舵机都在线")
        print("=============舵机正在回到初始位置=============")
        self.set_position(self.servo_id_1, 0, 50, 50)
        self.read(self.servo_id_1)
        print("舵机1已初始化成功")
        self.set_position(self.servo_id_2, 0, 50, 50)
        self.read(self.servo_id_2)
        print("舵机2已初始化成功")
        print("=============舵机初始化内容全部完成=============")

    def ping(self, servo_id):
        """
        检查舵机是否在线
        :param servo_id: 舵机ID
        :return: scs_comm_result == COMM_SUCCESS and scs_error == 0
        """
        model_number, scs_comm_result, scs_error = self.packetHandler.ping(servo_id)
        return scs_comm_result == COMM_SUCCESS and scs_error == 0

    def set_position(self, scs_id, position, speed, acc):
        """
        舵机运动到指定位置
        :param pos1: 第一个舵机的位置
        :param pos2: 第二个舵机的位置
        :param speed: 舵机运动速度
        :param acc: 运动加速度
        :return: scs_comm_result == COMM_SUCCESS and scs_error == 0
        """
        scs_comm_result, scs_error = self.packetHandler.WritePosEx(scs_id, position, speed, acc)
        if scs_comm_result != COMM_SUCCESS:
            print("%s" % self.packetHandler.getTxRxResult(scs_comm_result))
        elif scs_error != 0:
            print("%s" % self.packetHandler.getRxPacketError(scs_error))
        self.read(scs_id)
        return scs_comm_result == COMM_SUCCESS and scs_error == 0

    def read(self, scs_id):
        """
        循环读取舵机当前位置，判断舵机是否到达指定位置
        :param scs_id: 舵机号
        :return: None
        """
        while True:
            scs_present_position, scs_present_speed, scs_comm_result, scs_error = self.packetHandler.ReadPosSpeed(
                scs_id)
            if scs_comm_result != COMM_SUCCESS:
                print(self.packetHandler.getTxRxResult(scs_comm_result))
            else:
                print("舵机已到达指定位置")
            if scs_error != 0:
                print(self.packetHandler.getRxPacketError(scs_error))

            moving, scs_comm_result, scs_error = self.packetHandler.ReadMoving(scs_id)
            if scs_comm_result != COMM_SUCCESS:
                print(self.packetHandler.getTxRxResult(scs_comm_result))
            if moving == 0:
                break
        return

    def main(self,scs_id):
        """
        舵机运动组流程，分别为控制吸盘伸缩的舵机1，控制四指夹爪张开闭合的舵机2
        :param scs_id: 舵机号
        :return:
        动作组流程:{
            假设舵机初始位置都为0,最大位置为1000
            1、伸出吸盘到最大位置（同时打开气泵（当作有气泵））sleep(2)
            2、缩回吸盘到夹爪可闭合位置
            3、夹爪闭合
            4、sleep(3)
            5、夹爪张开
            6、吸盘伸出到最大位置 (后关闭气泵) sleep(2)
            7、吸盘缩回到初始位置
            动作流程完成
        """
        # 伸出吸盘到最大位置

        # 缩回吸盘到闭爪位置

        # 夹爪闭合

        # 夹爪张开

        # 吸盘伸出到最大位置

        # 吸盘缩回到初始位置

        pass


if __name__ == '__main__':
    app.run(host='0.0.0.0', port=6379, debug=False)
